#ifndef __MOTION_CONTROLLER_H_
#define __MOTION_CONTROLLER_H_


#include <ros/ros.h>
#include <motion_controller/bridge/planner_bridge.h>
#include <motion_controller/bridge/sensor_bridge.h>
#include <motion_controller/controller/controller_base.h>
#include <motion_controller/controller/controller_p2p.h>
#include <opencv2/opencv.hpp>
using namespace cv;

class MotionController{
    public:
        MotionController(tf2_ros::Buffer* tf_buffer);
        ~MotionController();

        // 雷达数据转换 sensor_msgs::LaserScan -> LidarScan2D
        bool Scan2LidarScan2D(LidarScan2D& lidatr_scan_2d);
        // motion_controller.cpp
        // 循环控制
        void Loop();

    private:
        // motion_callback.cpp
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGridConstPtr& msg);
    
    private:
        ros::NodeHandle __nh;
        ros::Subscriber __map_sub;
        ros::Subscriber __rviz_pose_sub;

    private: 
        boost::shared_ptr<const geometry_msgs::PoseStamped> __goal_pose_ptr; 
        boost::shared_ptr<ControllerBase> __controller_ptr;
        boost::shared_ptr<const nav_msgs::OccupancyGrid> __map_ptr;
        Map2D __map_2d; 

        boost::shared_ptr<PlannerBridge> __planner_bridge_ptr;
        boost::shared_ptr<SensorBridge> __sensor_bridge_ptr;
        float __robot_radius;       //机器人半径
};

#endif